import json
import copy
from typing import Dict, List, Any

try:
    from gglobal import logger
except ImportError:
    import logging
    logger = logging.getLogger(__name__)


def convert_obstacle_array_to_marker_array(obstacle_data: Dict[str, Any]) -> Dict[str, Any]:
    """
    将/lidar_perception/obstacle_array数据转换为markarray格式
    
    Args:
        obstacle_data: 原始的obstacle_array数据
        
    Returns:
        转换后的markarray格式数据
    """
    try:
        # 提取header信息
        header = obstacle_data.get("header", {})
        obstacle_array = obstacle_data.get("obstacle_array", [])
        
        # 创建markarray数据结构
        marker_array = {
            "markers": []
        }
        
        # 为每个障碍物创建一个marker
        for i, obstacle in enumerate(obstacle_array):
            # 提取shape作为points
            points = obstacle.get("shape", [])
            
            # 提取obs_source作为text
            obs_source = obstacle.get("obs_source", 0)
            
            # 创建marker
            marker = {
                "header": {
                    "stamp": header.get("stamp", {"sec": 0, "nanosec": 0}),
                    # "frame_id": header.get("frame_id", "obstacles")
                    "frame_id": "map"
                },
                "ns": "obstacle_convex_hull",
                "id": i,
                "type": 4,  # LINE_STRIP
                "action": 0,  # ADD
                "pose": {
                    "position": {"x": 0.0, "y": 0.0, "z": 0.0},
                    "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
                },
                "scale": {"x": 0.05, "y": 0.01, "z": 0.01},
                "color": {
                    # "r": 1.0 if obs_source == 1 else 0.0,  # 根据obs_source设置颜色
                    # "g": 1.0 if obs_source == 16 else 0.0,
                    "r": 0.0,
                    "g": 1.0,
                    "b": 0.0,
                    "a": 0.8
                },
                "lifetime": {"sec": 0, "nanosec": 0},  # 永久显示
                # "points": points,
                "points":[{"x": item["x"], "y": item["y"], "z": 0} for item in points],
                "colors": [],
                "text": str(obs_source)
            }
            
            marker_array["markers"].append(marker)
        
        logger.debug(f"成功转换obstacle_array到obstacle_convex_hull")
        return marker_array
        
    except Exception as e:
        logger.error(f"转换obstacle_array到marker_array时出错：{e}", exc_info=True)
        # 返回空的marker_array
        return {"markers": []}


def convert_obstacle_array_to_text_marker_array(obstacle_data: Dict[str, Any]) -> Dict[str, Any]:
    """
    将/lidar_perception/obstacle_array数据转换为文字类型的markarray格式
    
    Args:
        obstacle_data: 原始的obstacle_array数据
        
    Returns:
        转换后的文字markarray格式数据
    """
    try:
        # 提取header信息
        header = obstacle_data.get("header", {})
        obstacle_array = obstacle_data.get("obstacle_array", [])
        
        # 创建markarray数据结构
        marker_array = {
            "markers": []
        }
        
        # 为每个障碍物创建一个文字marker
        for i, obstacle in enumerate(obstacle_array):
            # 提取mbr的center_point作为文字位置
            mbr = obstacle.get("mbr", {})
            center_point = mbr.get("center_point", {"x": 0.0, "y": 0.0, "z": 0.0})
            
            # 提取障碍物信息
            vel = obstacle.get("vel", 0.0)
            status = obstacle.get("status", 1)
            obs_source = obstacle.get("obs_source", 0)
            # 字段的bit位类型
            src_meanings = {
                0: "laser_down",
                1: "front_cam",
                2: "rear_realsense",
                3: "front_mid360",
                4: "32_line_laser",
                5: "rear_mid360",
                6: "16_line_laser",
                7: "reasence_rgb",
                8: "arm_laser",
            }
            src_sources = [name for bit, name in src_meanings.items() if obs_source & (1 << bit)]
            
            # 构建文字内容
            status_text = "Static" if status == 1 else "Dynamic"
            text_content = f"vel: {vel:.2f}\ntype: {status_text}\nsenser: {src_sources}"
            
            # 创建文字marker
            marker = {
                "header": {
                    "stamp": header.get("stamp", {"sec": 0, "nanosec": 0}),
                    "frame_id": "map"
                },
                "ns": "obstacle_info",
                "id": i,
                "type": 9,  # TEXT_VIEW_FACING
                "action": 0,  # ADD
                "pose": {
                    "position": {
                        "x": center_point.get("x", 0.0),
                        "y": center_point.get("y", 0.0),
                        "z": center_point.get("z", 0.0)
                    },
                    "orientation": {"x": 0.0, "y": 0.0, "z": 0.0, "w": 1.0}
                },
                "scale": {"x": 0.0, "y": 0.0, "z": 0.15},
                "color": {
                    "r": 1.0,
                    "g": 1.0,
                    "b": 1.0,
                    "a": 1.0
                },
                "lifetime": {"sec": 0, "nanosec": 0},  # 永久显示
                "points": [],
                "colors": [],
                "text": text_content
            }
            
            marker_array["markers"].append(marker)
        
        logger.debug(f"成功转换obstacle_array为文字marker，包含{len(obstacle_array)}个障碍物")
        return marker_array
        
    except Exception as e:
        logger.error(f"转换obstacle_array到文字marker_array时出错：{e}", exc_info=True)
        # 返回空的marker_array
        return {"markers": []}


def process_lidar_perception_obstacle_array(msg_dict: Dict[str, Any]) -> Dict[str, Any]:
    """
    处理/lidar_perception/obstacle_array话题的数据清洗功能
    
    Args:
        msg_dict: 原始消息字典
        
    Returns:
        转换后的markarray数据
    """
    try:
        logger.debug("开始处理/lidar_perception/obstacle_array数据")
        
        # 转换为markarray格式
        marker_array_data = convert_obstacle_array_to_marker_array(msg_dict)
        
        return marker_array_data
        
    except Exception as e:
        logger.error(f"处理/lidar_perception/obstacle_array数据时出错：{e}", exc_info=True)
        return {"markers": []}


def process_lidar_perception_obstacle_array_info(msg_dict: Dict[str, Any]) -> Dict[str, Any]:
    """
    处理/lidar_perception/obstacle_array/info话题的数据清洗功能
    
    Args:
        msg_dict: 原始消息字典
        
    Returns:
        转换后的文字markarray数据
    """
    try:
        logger.debug("开始处理/lidar_perception/obstacle_array/info数据")
        
        # 转换为文字markarray格式
        marker_array_data = convert_obstacle_array_to_text_marker_array(msg_dict)
        
        return marker_array_data
        
    except Exception as e:
        logger.error(f"处理/lidar_perception/obstacle_array/info数据时出错：{e}", exc_info=True)
        return {"markers": []}